#ifndef __TINY_TEST__

#include "vbb_GPS.h"
#include <math.h>
#include "vbb_FLASHMEM.h"
#include "vbb_Header.h"
#include "fw/vbb_RTC.h"
#include "vbb_Storage.h"
#include "vbb_Sensor.h"
#include "vbb_GSM.h"
#include "vbb_FirmwareUtils.h"
#include <stdlib.h>

extern VBB_RTC rtc;
extern VBB_FlashMem flashMem;
extern VBB_Storage storage;
extern VBB_Sensor sensor;

extern bool cab_occupied;
extern unsigned int GSM_SignalQuality;

VBB_GPS gps;

void vbb_GPS_Init(void)
{
    if (vbb_DeviceModel() == BA01)
        vbb_USART_Init(&(gps.usart), USART3, 9600, 3, gps.buffer, 4096);    
    else        
        vbb_USART_Init(&(gps.usart), USART3, 19200, 3, gps.buffer, 4096);    
}

bool vbb_GPS_OnMoving(void)
{
    return (gps.vel_int > 0 || sensor.Velocity_Int > 0);
}

unsigned int GPS_ParseMessage(const char * Buffer, unsigned char * Offsets)
{
    unsigned int count = 1;    
    uint8_t i = 0;
    Offsets[0] = 0;
    
    while (Buffer[0] != '\0')
    {        
        Buffer++;        
        i++;
        
        if (Buffer[0] == '\0')
            break;

        if (Buffer[0] == ',')
        {
            Offsets[count] = i + 1;
            count++;
        }
    }
    
    return count;
}

unsigned int GPS_GetInteger(const char * Str, int Length)
{
    char numStr[32];
    int32_t retVal = 0;
    int16_t i;
    for (i = 0; i < Length; i++)
    {
        if ((Str[i] >= '0' && Str[i] <= '9'))
            numStr[i] = Str[i];
        else
            break;
    }    
    numStr[i] = '\0';    
    retVal = atoi(numStr);
    return retVal;
}

unsigned int GPS_GetFraction(const char * Str, int Length)
{
    char numStr[32];
    int32_t retVal = 0;
    int16_t i;
    
    while (Str[0] >= '0' && Str[0] <= '9')
        Str++;
    
    if (Str[0] == '.')
    {
        Str++;
        for (i = 0; i < Length; i++)
        {
            if ((Str[i] >= '0' && Str[i] <= '9'))
                numStr[i] = Str[i];
            else
            {
                numStr[i] = '0';
            }
        }    
        numStr[i] = '\0';    
        retVal = atoi(numStr);
        return retVal;
    }
    else
    {
        return 0;
    }
}

void GPS_MessageReformat(char * Message)
{
    int len = vbb_GetLen(Message);
    int i = 0;
    
    while (Message[len - 1] == '\r' || Message[len - 1] == '\n')
    {
        Message[len - 1] = 0;
        len--;
    }
    
    for (i = 0; i < len; i++)
        if (Message[i] == '\r' || Message[i] == '\n')
            Message[i] = 32;
}

unsigned char offsets[32];
unsigned int LastGenerateMessage;
unsigned int LastGenerateMiniMessage;

unsigned char body_msg[64];

int GPS_Hex2Dec(unsigned char c)
{
    if (c >= 'A')
        c = 10 + (c - 'A');
    else if (c >= '0' && c <= '9')
        c = c - '0';
    return c;
}

bool GPS_CheckSum(const char * Message)
{
    int i = 0;
    int len = vbb_GetLen(Message);
    unsigned char chksum = 0;
    unsigned char tchksum = 0;
    
    while (Message[len - 1] == '\r' || Message[len - 1] == '\n')
        len--;    
    
    if (Message[len - 1] != 0 && Message[len - 2] != 0 && Message[len - 3] == '*')
    {
        chksum = GPS_Hex2Dec(Message[len - 2]) * 16;
        chksum += GPS_Hex2Dec(Message[len - 1]);

        for (i = 0; i < len - 3; i++)
        {
            if (Message[i] != '*' && Message[i] != '$')
                tchksum ^= Message[i];
        }
        
        return tchksum == chksum;
    }
    else
        return FALSE;    
}

void GPS_LedSignal_NoGPS(void)
{
    vbb_LedSwitch(TRUE, FALSE);
    vbb_DelayMillis(100);
    vbb_LedSwitch(FALSE, TRUE);
    vbb_DelayMillis(100);
    vbb_LedSwitch(TRUE, FALSE);
    vbb_DelayMillis(100);
    vbb_LedSwitch(FALSE, TRUE);
    vbb_DelayMillis(100);
    vbb_LedSwitch(TRUE, FALSE);
    vbb_DelayMillis(100);
    vbb_LedSwitch(FALSE, TRUE);
    vbb_DelayMillis(100);
    vbb_LedSwitch(TRUE, FALSE);
    vbb_DelayMillis(100);
}

extern int communication_rate_int;
extern int communication_rate_fra;

extern unsigned int supply_voltage;

void GPS_GotMessage(const char * Message)
{
    const char * buff = Message;
    unsigned char SignalActive;
    unsigned int Lat_Int;
    unsigned int Lat_Fra;
    unsigned int Long_Int;
    unsigned int Long_Fra;
    unsigned int Vel_Int;
    unsigned int Vel_Fra;
    
    unsigned int kmCount_Int;
    unsigned int kmCount_Fra;
    
    unsigned int TmpVel;
    
    int msgPerHour;
    
    //Message = "$GPRMC,123519,A,2059.836,N,10547.732,E,022.45,084.4,230394,003.1,W*6A\r\n";
   
    buff = Message;  
    
    buff = vbb_FindStr(buff, "$GPRMC");
    buff += 3;
    if (buff && vbb_RTC_GetSec() - LastGenerateMessage >= 1) 
    {      
        if (GPS_CheckSum(Message))
        {
            //vbb_CONSOLE_Printf(&console, "%s\r\n", Message);
            //vbb_OUTPUT_PrintDebug(Message);
            //vbb_OUTPUT_Print("\r\n");
            
            if (vbb_DebugMode())
            {
                vbb_OUTPUT_PrintDebug(Message);
                vbb_OUTPUT_Print("\r\n");
            }
            
            GPS_ParseMessage(Message, offsets);                
            
            //Update GPS Signal
            buff = Message + offsets[2];
            SignalActive = (buff[0] == 'A');
            
            /*
            if (!SignalActive)
                GPS_LedSignal_NoGPS();
            */
            
            //Update GPS Coordination
            buff = Message + offsets[3];
            Lat_Int = GPS_GetInteger(buff, 14);
            Lat_Fra = GPS_GetFraction(buff, 3);
            buff = Message + offsets[5];
            Long_Int = GPS_GetInteger(buff, 14);
            Long_Fra = GPS_GetFraction(buff, 3);
            
            Lat_Fra = (Lat_Int % 100) * 1000 + Lat_Fra;
            Lat_Int = (Lat_Int / 100);
            
            Long_Fra = (Long_Int % 100) * 1000 + Long_Fra;
            Long_Int = (Long_Int / 100);
            
            Lat_Fra = Lat_Fra * 100 / 60;
            Long_Fra = Long_Fra * 100 / 60;

            buff = Message + offsets[7];
            Vel_Int = GPS_GetInteger(buff, 14);
            Vel_Fra = GPS_GetFraction(buff, 2);
                    
            Vel_Int = ((Vel_Int * 100 + Vel_Fra) * 1852) / 1000;
            Vel_Fra = Vel_Int % 100;
            Vel_Int = Vel_Int / 100;
            
            TmpVel = Vel_Int * 100 + Vel_Fra;
            TmpVel += gps.vel_int * 100 + gps.vel_fra;
            TmpVel /= 2; //Filter the velocity by averaging, cleaning noise
            
            Vel_Fra = TmpVel % 100;
            Vel_Int = TmpVel / 100;
            
            gps.active = SignalActive;
            gps.lat_int = Lat_Int;
            gps.lat_fra = Lat_Fra;
            gps.long_int = Long_Int;
            gps.long_fra = Long_Fra;
            gps.vel_int = Vel_Int;
            gps.vel_fra = Vel_Fra;
            
            vbb_RTC_Update(&rtc);
            
            if (vbb_GPS_OnMoving() || sensor.EngineStarted || cab_occupied)    
                msgPerHour = storage.config.MessagePerHour_Move;
            else
                msgPerHour = storage.config.MessagePerHour_Idle;

            if (vbb_DebugMode())
                msgPerHour = storage.config.MessagePerHour_Move;
            
            // 2013.10.09 Increase resolution of Kilometer Count from Pulse
            kmCount_Int = (sensor.CollectedPulse * 100) / storage.config.PulseOnKm;                
            kmCount_Fra = kmCount_Int % 100;
            kmCount_Int = kmCount_Int / 100;
                
            if (vbb_RTC_GetSec() - LastGenerateMiniMessage > 3600 / (msgPerHour * 8)) // 2013.10.08 Increase number of $MINI twice
            {                
                vbb_FLASHMEM_PushMessageF("$MINI,%d,%s,%s,%02d%02d%02d,%02d%02d%02d,%d,%d.%05d,%d.%05d,%d.%02d,%u,%d.%02d,%d.%02d,%01d%01d%01d%01d,%d,%d",
                    VERSION,
                    storage.config.OwnerId,
                    storage.config.VehicleId,                    
                    rtc.hour,
                    rtc.minute,
                    rtc.second,
                    rtc.day,                
                    rtc.month,
                    rtc.year % 100,
                    SignalActive,
                    Lat_Int,
                    Lat_Fra,
                    Long_Int,
                    Long_Fra,
                    Vel_Int,
                    Vel_Fra,
                    sensor.CollectedPulse,
                    kmCount_Int,
                    kmCount_Fra,
                    sensor.Velocity_Int,
                    sensor.Velocity_Fra,
                    cab_occupied,
                    sensor.EngineStarted,
                    sensor.DoorClosed,
                    sensor.ACPowered,
                    GSM_SignalQuality,
                    supply_voltage);
                LastGenerateMiniMessage = vbb_RTC_GetSec();
            }
            /*
            else
            {
                vbb_FLASHMEM_PushMessageF("$GPS,%d,%s,%s,%02d%02d%02d,%02d%02d%02d,%d,%d.%05d,%d.%05d,%d.%02d",
                    VERSION,
                    storage.config.OwnerId,
                    storage.config.VehicleId,                    
                    rtc.hour,
                    rtc.minute,
                    rtc.second,
                    rtc.day,                
                    rtc.month,
                    rtc.year % 100,
                    SignalActive,
                    Lat_Int,
                    Lat_Fra,
                    Long_Int,
                    Long_Fra,
                    Vel_Int,
                    Vel_Fra);
            }
            */
        }
        else
        {
            vbb_OUTPUT_Print("DEBUG: Invalid GPRRMC Message.\r\n");
            vbb_OUTPUT_PrintDebug(Message);
            vbb_OUTPUT_Print("\r\n");
        }
        
        LastGenerateMessage = vbb_RTC_GetSec();
    }
}

void vbb_GPS_Flush(void)
{
    uint8_t c;

    VBB_DAT_Queue * queue = &(gps.usart.rBuffer);
    VBB_Message * msg = &(gps.msg);
    
    while (queue->head < queue->tail)
    {
        c = queue->buffer[queue->head & queue->bufferSizeMinusOne];
        queue->head++;        
        
        if (msg->length == 0)		
        {            
            if (c == '$')
                vbb_Message_AddByte(msg, c);
            else
                vbb_Message_Init(msg);
		}
        else if (msg->length == 1)
        {
            if (c == 'G')
                vbb_Message_AddByte(msg, c);
            else
                vbb_Message_Init(msg);
        }
        else if (msg->length == 2)
        {
            if (c == 'P')
                vbb_Message_AddByte(msg, c);
            else
                vbb_Message_Init(msg);
        }
        else if (msg->length == 3)
        {
            if (c == 'R')
                vbb_Message_AddByte(msg, c);
            else
                vbb_Message_Init(msg);
        }
        else if (msg->length == 4)
        {
            if (c == 'M')
                vbb_Message_AddByte(msg, c);
            else
                vbb_Message_Init(msg);
        }
        else if (msg->length == 4)
        {
            if (c == 'C')
                vbb_Message_AddByte(msg, c);
            else
                vbb_Message_Init(msg);
        }
		else if (msg->length > 4)
		{
			vbb_Message_AddByte(msg, c);
			
            if (c == '\r')
            {                
                vbb_Message_AddByte(msg, 0);                
                vbb_StrCpy(gps.finalMsg.buff, msg->buff, vbb_GetLen(msg->buff));
                vbb_Message_Init(msg);                
            }
		}
    }    
    
    if (gps.finalMsg.buff[0])
    {
        GPS_MessageReformat(gps.finalMsg.buff);                
        GPS_GotMessage(gps.finalMsg.buff);
    }
}

#endif
